/*
 * @Author: your name
 * @Date: 2020-02-23 12:42:59
 * @LastEditTime: 2020-03-05 23:17:23
 * @LastEditors: Please set LastEditors
 * @Description: In User Settings Edit
 * @FilePath: /platform_controller_server/include/platform_controller_server/patrol.h
 */
#ifndef PATROL_H
#define PATROL_H

#include <iostream>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <fstream>
#include <cstdio>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int8.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>

#include <sensor_msgs/Image.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>


#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <platform_controller_msgs/IsArrivedTarget.h>
#include <platform_controller_msgs/PatrolTask.h>
#include <platform_controller_msgs/NavigationPoints.h>
#include <platform_controller_msgs/Patients.h>
#include <tf/transform_listener.h>

//MoveBASE
#include <move_base_msgs/MoveBaseActionFeedback.h>
#include <move_base_msgs/MoveBaseActionResult.h>

//MoveBase
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

//JSON
#include "rapidjson/document.h"
#include "rapidjson/writer.h"
#include "rapidjson/stringbuffer.h"
#include "rapidjson/prettywriter.h" // for stringify JSON
#include "rapidjson/filereadstream.h"

//CSV file
#include "platform_controller_server/CSVparser.h"

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include <Eigen/Core>

using namespace rapidjson;
using namespace std;


#define LOAD_PATROL_POINTS     1
#define LOAD_PATIENTS          2
#define LAUNCH_AMCL            3
#define CLOSE_AMCL             4
#define LAUNCH_ROSBAG_RECORD   5
#define CLOSE_ROSBAG_RECORD    6
#define LAUNCH_GMAPPING        7
#define CLOSE_GMAPPING         8
#define BEGIN_PATROL           9
#define PATROL_TASK_SUCCESS    10


class Patrol{

public:
	typedef struct PatientInfo{
		std::string name;          //姓名
		std::string sex;           //性别
		int room_num;              //房间号
		void show()
		{
			std::cout<<"name: "<<name<<" sex: "<<sex<<" room_num: "<<room_num<<std::endl;
		}
	}PatientInfo;

	typedef struct OperationResponse{
		uint8_t operation_type;
		uint8_t result;
		std::string verbose;
		
		std::string toJsonString()
		{
			rapidjson::Document doc;
			doc.SetObject();
			rapidjson::Document::AllocatorType& allocator = doc.GetAllocator();

			doc.AddMember("operation_type",operation_type,allocator);
			doc.AddMember("result",result,allocator);

			rapidjson::Value verboseObj(rapidjson::kStringType);
			verboseObj.SetString(verbose.c_str(),verbose.length(),allocator);
			doc.AddMember("verbose",verboseObj,allocator);

			rapidjson::StringBuffer s;
			rapidjson::Writer<rapidjson::StringBuffer> writer(s);
			doc.Accept(writer);
			
			// return std::string("");
			return std::string(s.GetString());
		}
	}OperationResponse;

	Patrol(const ros::NodeHandle& nh,const ros::NodeHandle& nh_private);
    ~Patrol();
	void setWaitSeconds(int seconds);                                   //设置等待时间
	platform_controller_msgs::NavigationPoint getCurrentTarget();                                 //获取当前导航的目标点
	void visuial_map();
	
protected:
	void onSingleCirclePatrolCallback(const std_msgs::Empty::ConstPtr& msg);
	void onGoalFeedbackCallback(const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg);
	void onGoalResultCallback(const move_base_msgs::MoveBaseActionResult::ConstPtr& msg);
	

	void onNavGridMapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map);
	void onLaserScanCallback(const sensor_msgs::LaserScan::ConstPtr& map);
private:

	

	ros::NodeHandle nh_,nh_private_;
	ros::Subscriber sub_enablePatrol,sub_loadNavigationPoints,sub_getPatrolCommand,sub_isArrivedTarget,sub_firstGlobalPath,sub_task_completed,sub_add_patrol_point,sub_save_patrol_point;
	ros::Subscriber sub_single_circle, sub_movebase_feedback,sub_movebase_result;
	ros::Subscriber sub_nav_map,sub_laser_scan; 
	ros::Subscriber sub_load_patients;
	
	ros::Publisher pub_currentTarget,pub_androidVoice,pub_cancelCurrentTarget;                                   //发布当前要前往的点
	ros::Publisher pub_tts;
	ros::Publisher pub_compose_img;
	ros::Publisher pub_patients;
	ros::Publisher pub_log_;

	ros::Publisher pub_patrol_task_completed_;

	ros::Publisher pub_operation_result_;

	ros::Timer sendTargetTimer_;

	cv::Mat  convertNavMap2Img(nav_msgs::OccupancyGrid::ConstPtr& occupancy_map);
	


    //void addPartolPoint(const platform_controller_msgs::NavigationPoint& partrolPoint);

    void timerEvent(const ros::TimerEvent&e);

	void enablePartol_callback(const std_msgs::Bool::ConstPtr &);
	
    void sendTarget();                                                            //发送新的目标点
	void onEnableOdomNavigationCallback(const std_msgs::Bool::ConstPtr& msg);
	
	void onAddPatrolPoint(const std_msgs::String::ConstPtr& msg);
	void onLoadPatrolPoints(const std_msgs::String::ConstPtr& msg);
	void onLoadCSVFile(const std_msgs::String::ConstPtr& msg);
	void onSavePatrolPoints(const std_msgs::String::ConstPtr& file);
	void onPatrolCommand_callback(const platform_controller_msgs::PatrolTask::ConstPtr &msg);
	void isArrivedTarget_callback(const platform_controller_msgs::IsArrivedTarget::ConstPtr &msg);
	void firstGlobalPath_callback(const nav_msgs::Path::ConstPtr &msg);
	void onTaskCompletedCallback(const std_msgs::Bool::ConstPtr &msg);

	std::string patient2Json(const platform_controller_msgs::Patient& patients);
	std::string patroPoint2Json(const platform_controller_msgs::NavigationPoint& point);

	std::vector<platform_controller_msgs::NavigationPoint> totalNavigationPoints_;            //全部导航点序列　　　　
	std::vector<platform_controller_msgs::NavigationPoint> patrolNavigationPoints_;           //当前导航点序列                                                     //当前目标点的id号
	platform_controller_msgs::NavigationPoint navigationPoint_;                               //当前目标点

	bool isFirstGlobalPath_;                                                      //是否是第一次接收到全局路径（以第一次接收到的全局路径来估计运动的时间）
	bool isEnablePatrol_;		                                                  //是否开启巡逻的模式　　　　　　　　　　　

	int time_;                                                                    //前往当前目标点预计需要的时间         
	int scale_;                                                                   //每一个全局路径点之间所需时间比例　　　　　　　　　　　　　　　　　　　　
	int rest_time_;	                                                              //前往当前目标点预计shengyu时间　　　　　　　　　　　　　　　　

	int patrol_mode_;                                                             //巡逻模式（随机，顺序）

	ros::Time sendTargetTime_;                                                    //发布目标点的时间
	ros::Time currentTime_;                                                       //当前时间
	ros::Time arrivedTime_;                                                       //到达时间
	ros::Duration waitSeconds_;                                                   //到达目标点后的等待时间　　

	bool patrol_go_flag_;    
	bool update_goal_; 
	bool sendTargetFlag_;    
	bool single_circle_flag_;         

	geometry_msgs::PoseStamped robot_pose_;
	bool location_status_;
	int hasMap_;

	std::string navigation_points_save_path_;
	std::string navigation_points_load_path_;


	std_msgs::String tts_notification_msg_;

	bool update_map_flag_;
	boost::mutex occupancy_map_mutex_;
	nav_msgs::OccupancyGrid::ConstPtr occupancy_map_ = nullptr;                                        //当前地图
	cv::Mat composed_map_img_;                                                     //融合激光雷达，位置，地图的信息图片

	boost::mutex laserscan_mutex_;
	sensor_msgs::LaserScan::ConstPtr laser_scan_ = nullptr;

	std::shared_ptr<csv::Parser> csv_parer_ptr_ = nullptr;
	platform_controller_msgs::Patients patientInfoVec_;                                     //病人信息列表
	std::string csv_save_path_;

	OperationResponse operation_response_;
};



#endif//PATROL_H